%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-14,'AbsTol',1e-22);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.


planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%% environment settings 
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
% OpenCRTBP_u(u)

%% L1
OpenCRTBP_u_SE(u); hold on;
load('SEL1_Lyapunov.mat')
for ii = 1:length(IC);
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:1e-2:T(ii)*2.01],IC(:,ii),options);
    S_save_L1{ii}=S';
    J_save_L1(ii) = jacobiConst(IC(1:3,ii),IC(4:6,ii),u);
end

cmap = colormap('parula'); % take your pick (doc colormap)
zmap = linspace( min(J_save_L1), max(J_save_L1), length(cmap));

for ii = 1:15:length(IC);
    color = interp1( zmap, cmap, J_save_L1(ii));
    plot3(S_save_L1{ii}(1,:),S_save_L1{ii}(2,:),S_save_L1{ii}(3,:), 'color',color,'Linestyle','-');    
end

caxis([min(zmap), max(zmap)])
cb = colorbar(); 
ylabel(cb,'Jacobi integral','FontSize',11,'Rotation',270)
cb.Label.Position(1) = 4.5;

%% L4
OpenCRTBP_u_SE(u); hold on;
load('SE_L4_Vertical.mat');
ind = 1;
plotAmount = 50;
for ii = 1:plotAmount:length(IC_L4_VL)
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:1e-2:T_L4_VL(ii)],IC_L4_VL(:,ii),options);
    S_save_VL{ind}=S';
    J_save_VL(ind) = jacobiConst(IC_L4_VL(1:3,ii),IC_L4_VL(4:6,ii),u);
    ind = ind +1;
end

load('SE_L4_L5_Lyapunov.mat')
for ii = 1:5:length(IC_L4)-5;
    [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:1e-2:T(ii)],IC_L4(:,ii),options);
    S_save_PL{ii}=S';
    J_save_PL(ii) = jacobiConst(IC_L4(1:3,ii),IC_L4(4:6,ii),u);
end
cmap = colormap('parula'); % take your pick (doc colormap)
zmap = linspace( 2.88, 3, length(cmap));

ind = 1;
for ii = 1:plotAmount:length(IC_L4_VL)
    color = interp1( zmap, cmap, J_save_VL(ind));
    plot3(S_save_VL{ind}(1,:),S_save_VL{ind}(2,:),S_save_VL{ind}(3,:), 'color',color);    
    ind = ind +1;
end
ind=182;
[t,S] = ode113(@(t,S)CR3BP_n(t,S,u),[0:1e-2:T_L4_VL(ind)],IC_L4_VL(:,ind),options);
S=S';
J_save_VL_temp = jacobiConst(S(1:3,1),S(4:6,1),u);
plot3(S(1,:),S(2,:),S(3,:), 'color','r','LineWidth',3,'LineStyle','-.');
ind = ind +1;


for ii = 1:5:length(IC_L4)-5;
    color = interp1( zmap, cmap, J_save_PL(ii));
    plot3(S_save_PL{ii}(1,:),S_save_PL{ii}(2,:),S_save_PL{ii}(3,:), 'color',color,'Linestyle','-.');    
end

caxis([min(zmap), max(zmap)])
cb = colorbar; 
ylabel(cb,'Jacobi integral','FontSize',11,'Rotation',270)
cb.Label.Position(1) = 4;
